/*
 * Copyright (c) 2011, Fredrik Häggström
 *
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the Luleå University of Technology nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
 * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include <tT.h>
#include <env.h>

#include "hsadc.h"
#include "pid.h"
#include "can.h"
#include "mcpwm.h"

uint32_t pid_print = 0;



env_result_t pid_run(pid_t *self, void *none)
{
	int32_t tempK = 0, tempI = 0, tempD = 0, tempsum = 0;
	
	//ENV_DEBUG("pid run\r\n");
	
	if ( mode == 1) {
	
	
		//self->e = ((self->r - ((*(self->input)) *8)) << 0);				// shift upp the error to do calc
		self->e = ((self->r - (hsadc.value[1]) *8) << 0);				// shift upp the error to do calc
		
		env_send(self->e);
		ENV_DEBUG("\r\n");
		
		tempK = self->e * self->K;
	
		if (self->Ti)
		{
			tempsum = self->Isum + tempK * self->dt;		// dt is in micro sec
		
			tempI = tempsum / (self->Ti * 1000);
		}
	
		if (self->Td)
		{
			tempD = tempK * self->Td * ( *(self->input) - self->prev );
		
			if (!self->dt)
			{
				ENV_DEBUG("PID Div with zero!\r\n");
				return 1;	
			}
			tempD = tempD / (self->dt);
		}
	
		self->output = (( (tempK / 10) + tempI + tempD ) >> 0);	// shift down for output.



		/* 	Set limits for the output and integrate only 
				when the integralpart is smaller that 'max' */
		if (self->output >= self->max) 
		{
		
			self->output = self->max;
			if ((tempI>>0) < self->max)
			{
				self->Isum = tempsum;
			}
		
		}
		else if (self->output <= self->min)
		{
		
			self->output = self->min;
			if ((tempI>>0) > self->min)
			{
				self->Isum = tempsum;
			}
		}
		else
		{
			self->Isum = tempsum;
		}
		

		
		TT_SYNC( self->target, self->function, (&self->output)); // run the targeted function with output as arg.
	
	
		//self->function( self->target, (void *)(&self->output));
	
		//env_send(hsadc.value[1]);
		//if (pid_print < 100) {
	//
	//		env_send(self->e);
	//		ENV_DEBUG(",");
	//		env_send(self->output);
	//		ENV_DEBUG(",");
	//		env_send(self->Isum);
	//		ENV_DEBUG("\r\n");
	//		pid_print++;
	//	}
	}
	
	//TT_WITHIN((CCLK/1000000) * self->dt, 10000, self, pid_run, TT_ARGS_NONE);
	//TT_WITHIN(1000000 , 10000, self, pid_run, TT_ARGS_NONE);
	
	return 0;
}

env_result_t pid_reset( pid_t *self, void *none)
{
	self->output = 0;
	self->prev = 0;
	self->Isum = 0;
	return 0;
}



env_result_t pid_decode( pid_t *self, int32_t data[2])
{
	ENV_DEBUG("pid_decode() ");

	switch (data[0] & 0x000000FF) {
		case 0: ENV_DEBUG("r ");
						if ( mode != 1 ) pid_reset(self, TT_ARGS_NONE);
						self->r = data[1];
						TT_SYNC(&mcpwm, mcpwm_enable, TT_ARGS_NONE); 
						mode = 1;
						break;
	
		case 1:	ENV_DEBUG("K ");
						self->K = data[1];
						break;
		
		case 2: ENV_DEBUG("Ti ");
						self->Ti = data[1];
						break;
						
		case 3: ENV_DEBUG("Td ");
						self->Td = data[1];
						break;
						
		case 4: ENV_DEBUG("dt ");
						self->dt = data[1];
						break;	
									
		case 5: ENV_DEBUG("max ");
						self->max = data[1];
						break;
						
		case 6: ENV_DEBUG("min ");
						self->min = data[1];
						break;
						
		case 7: pid_reset(self, TT_ARGS_NONE);
						break;
																	
		default:
						break;
	}	
	env_send(data[1]);
	ENV_DEBUG("\r\n");


	return 0;
}

